package xia;
import com.google.gson.Gson;
import com.jilk.ros.rosapi.message.TypeDef;
import com.zlp.mymapviewproject.mymapview.MyMapPointWithTitleView;
import java.util.ArrayList;
import java.util.regex.Pattern;

import org_rosjava.ros.android.android_tutorial_map_viewer.My_map;
import xia.Bean.Bean_from_slam_mark;

import static java.lang.Math.PI;


public class Ros_Connect {
    static Thread mylisten_connect_Thread;
    static String ros_ip_addr="";
    public static boolean keep_while_flag=true;
    static boolean is_need_continue_connect_falg = true;
    static TypeDef[] typeDef;
    static  int my_continue_sleep_timr=30*1000;
    private static boolean checkIP(String str) {
        Pattern pattern = Pattern
                .compile("^((\\d|[1-9]\\d|1\\d\\d|2[0-4]\\d|25[0-5]"
                        + "|[*])\\.){3}(\\d|[1-9]\\d|1\\d\\d|2[0-4]\\d|25[0-5]|[*])$");
        return pattern.matcher(str).matches();
    }
    public static void send_cmd(String cmd)
    {
        if (My_map.getinstance() != null)
        {
            try {
                if (!cmd.isEmpty()) {
                    if (cmd.equals("wake_up")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("cancel");
                    } else if (cmd.equals("继续走")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("cancel end");
                    }
                    else if (cmd.equals("暂停")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("cancel");
                    }
                    else if (cmd.equals("去下一个地方")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("cancel end");
                    } else if (cmd.equals("空闲模式")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("idle");
                    } else if (cmd.equals("巡逻模式")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("patrol");
                    } else if (cmd.equals("单点导航")) {
                        //My_map.getinstance().myRos_sendTextPublish.sendStringData("simplenav:379,100,0,0,0,0,1.0:name");
                    } else if (cmd.equals("充电模式")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("autocharge");
                    } else if (cmd.equals("停止移动")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("idle");
                    } else if (cmd.equals("厨房")) {
                        //My_map.getinstance().myRos_sendTextPublish.sendStringData("simplenav:379,100,0,0,0,0,1.0:name");
                    } else if (cmd.equals("mark_result_request")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("mark_result_request");
                    } else if (cmd.equals("mapping:start")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("mapping:start");
                    } else if (cmd.equals("mapping:save")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("mapping:save");
                    } else if (cmd.equals("mapping:close")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("mapping:close");
                    }
                    else if (cmd.equals("shutdown")) {
                        My_map.getinstance().myRos_sendTextPublish.sendStringData("shutdown");
                    }
                }
            } catch (Exception e) {
                e.printStackTrace();
            }
        }
    }
    public static void parseWork_mode_result(final String data) {
        try {
            if(data.equals("idle:ing")||data.equals("patrol:ing")||data.contains(":ing"))
            {
                if(data.equals("mapping:start:ing"))
                {
                    My_map.getinstance().is_at_mapping = true;
                    Ros_Connect.send_cmd("mark_result_request");
                    My_map.getinstance().is_first_wait_map = true;
                    My_map.getinstance().runOnUiThread(new Runnable() {
                        @Override
                        public void run() {
                        }
                    });
                    return;
                }
                else if (data.contains("cancel:ing"))//
                {
                    My_map.getinstance().is_at_move=false;
                    My_map.getinstance().runOnUiThread(new Runnable() {
                        @Override
                        public void run() {
                            My_map.getinstance().PlaySound(5);
                            My_map.getinstance().show_status.setText("导航模式(暂停)");
                        }
                    });

                    return;
                }
                else if (data.contains("cancel end:ing"))//
                {
                    My_map.getinstance().is_at_move=true;
                    My_map.getinstance().runOnUiThread(new Runnable() {
                        @Override
                        public void run() {
                            My_map.getinstance().PlaySound(6);
                            My_map.getinstance().show_status.setText("导航模式");
                        }
                    });
                    return;
                }
                Ros_Connect.send_cmd("mark_result_request");//同步底盘数据库表格
                return;
            }
            else if(data.equals("mark:patrol_line_coordinate:succeeded")||data.equals("mark:coordinate_add:succeeded")||data.equals("mark:coordinate_del:succeeded")||(data.contains("simplenav:")&&data.contains("succeeded"))||(data.contains("simplenav:")&&data.contains("timeout")))
            {
                if(data.equals("mark:coordinate_add:succeeded"))
                    My_map.getinstance().mark_coordinate_add_succeeded=true;
                if(data.equals("mark:coordinate_del:succeeded"))
                    My_map.getinstance().mark_coordinate_del_succeeded=true;
                if(data.equals("mark:patrol_line_coordinate:succeeded"))
                    My_map.getinstance().mark_line_change_succeeded=true;
                if(data.contains("simplenav:")&&data.contains("succeeded")) {
                    String[] sArray = data.split(":");
                    final String succeeded_name = sArray[1];
                    if (!succeeded_name.equals("厨房"))
                    {
                        My_map.getinstance().runOnUiThread(new Runnable() {
                            @Override
                            public void run() {
                                if (My_map.getinstance().show_goto != null) {

                                    My_map.getinstance().show_goto.setText("已经到达" + succeeded_name + "餐桌");
                                }
                                My_map.getinstance().PlaySound(2);
                            }
                        });

                    }
                    else {
                        My_map.getinstance().runOnUiThread(new Runnable() {
                            @Override
                            public void run() {
                                if (My_map.getinstance().show_goto != null) {

                                    My_map.getinstance().show_goto.setText("我回来啦，请吩咐" );
                                }
                                My_map.getinstance().PlaySound(8);
                            }
                        });
                    }
                }
                if(data.contains("simplenav:")&&data.contains("timeout"))
                {
                    String[] sArray = data.split(":");
                    final String timeout_name=sArray[1];
                    My_map.getinstance().runOnUiThread(new Runnable() {
                        @Override
                        public void run() {
                            if(My_map.getinstance().show_goto!=null) {

                                My_map.getinstance().show_goto.setText("前往" + timeout_name + "超时");
                            }
                            My_map.getinstance().PlaySound(4);
                        }
                    });
                }
                Ros_Connect.send_cmd("mark_result_request");//同步底盘数据库表格
                return;
            }
            else if(data.contains("{"))
            {
                My_map.getinstance().runOnUiThread(new Runnable() {
                    @Override
                    public void run() {
                        markjsondata_to_points(data);
                    }
                });
                return;
            }
            else if(data.equals("mapping:save:succeeded"))
            {
                My_map.getinstance().is_at_mapping = false;
                Ros_Connect.send_cmd("mark_result_request");
                My_map.getinstance().is_first_wait_map = true;
                My_map.getinstance().runOnUiThread(new Runnable() {
                    @Override
                    public void run() {
                    }
                });
                return;
            }
            else if(data.equals("mapping:close:succeeded"))
            {
                My_map.getinstance().is_at_mapping = false;
                Ros_Connect.send_cmd("mark_result_request");
                My_map.getinstance().is_first_wait_map = true;
                My_map.getinstance().runOnUiThread(new Runnable() {
                    @Override
                    public void run() {
                    }
                });
                return;
            }
            else if(data.equals("shutdown:succeeded"))
            {
                My_map.getinstance().is_at_mapping = false;
                My_map.getinstance().runOnUiThread(new Runnable() {
                    @Override
                    public void run() {
                    }
                });
                return;
            }
            else if(data.equals("wall:add:succeeded"))
            {
                Ros_Connect.send_cmd("mark_result_request");
                return;
            }
            else if(data.equals("wall:del:succeeded"))
            {
                Ros_Connect.send_cmd("mark_result_request");
                return;
            }
            String sss[] = data.split(":");
            if(sss.length==3)
            {
                String speek_value = "到达" + sss[2];
            }
            return;
        } catch (Exception e) {
            e.printStackTrace();
        }
    }
    static void markjsondata_to_points(String json_value)
    {
        try {
            Gson a=new Gson();
            Bean_from_slam_mark get_slam_mark_value = a.fromJson(json_value, Bean_from_slam_mark.class);
            My_map.getinstance().mark_coordinatejsondata_last=get_slam_mark_value;
            if(My_map.getinstance().is_first_start_wait_mark_result_request == true)
            {
                if(My_map.getinstance().mark_coordinatejsondata_last!=null)
                {
                    My_map.getinstance().runOnUiThread(new Runnable() {
                        @Override
                        public void run() {
                            My_map.getinstance().show_layout_button();
                        }
                    });
                }
            }
            My_map.getinstance().is_first_start_wait_mark_result_request = false;
            My_map.getinstance().mode = get_slam_mark_value.work_mode;
            if (My_map.getinstance().mode.equals("idle")) {
                My_map.getinstance().user_mode = "idle";
                My_map.getinstance().is_at_mapping = false;
                My_map.getinstance().runOnUiThread(new Runnable() {
                    @Override
                    public void run() {
                            My_map.getinstance().show_status.setText("空闲模式");
                        My_map.getinstance().is_at_move = false;
                    }
                });
            } else if (My_map.getinstance().mode.equals("simplenav")) {
                My_map.getinstance().user_mode = "simplenav";
                My_map.getinstance().is_at_mapping = false;
                My_map.getinstance().runOnUiThread(new Runnable() {
                    @Override
                    public void run() {
                        My_map.getinstance().show_status.setText("导航模式");
                    }
                });
            }
            if(My_map.getinstance().is_first_Layout_show==true)
                return;
            if (My_map.getinstance().is_switch_ros_app_mapView_big == true||My_map.getinstance().is_need_edit_point_map_show_flag==false)
                return;
            if(My_map.getinstance().is_at_mapping==false)
            {
                    ArrayList<MyMapPointWithTitleView> mapPoints_from_slam = new ArrayList<>();
                    for (int i = 0; i < get_slam_mark_value.mark_coordinate.size(); i++) {
                        Float coordinate_x = get_slam_mark_value.mark_coordinate.get(i).coordinate.get(0);
                        Float coordinate_y = get_slam_mark_value.mark_coordinate.get(i).coordinate.get(1);
                        String name = get_slam_mark_value.mark_coordinate.get(i).name;
                        Float coordinate_dir_x = get_slam_mark_value.mark_coordinate.get(i).coordinate.get(3);
                        Float coordinate_dir_y = get_slam_mark_value.mark_coordinate.get(i).coordinate.get(4);
                        Float coordinate_dir_z = get_slam_mark_value.mark_coordinate.get(i).coordinate.get(5);
                        Float coordinate_dir_w = get_slam_mark_value.mark_coordinate.get(i).coordinate.get(6);
                        final Double angle = Math.atan2(2 * (coordinate_dir_w * coordinate_dir_z + coordinate_dir_x * coordinate_dir_y), 1 - 2 * (coordinate_dir_z * coordinate_dir_z + coordinate_dir_y * coordinate_dir_y));
                        double my_angle = (angle / PI) * 180;
                        my_angle = -my_angle + 90;
                        if (My_map.getinstance().resolution != null) {
                            if (My_map.getinstance().resolution != 0d && My_map.getinstance().map_hight != 0) {
                                Float yy = (float) (My_map.getinstance().map_hight - (coordinate_y - My_map.getinstance().resolution_position_y) / My_map.getinstance().resolution);
                                Float xx = (float) ((coordinate_x - My_map.getinstance().resolution_position_x) / My_map.getinstance().resolution);
                                mapPoints_from_slam.add(new MyMapPointWithTitleView(My_map.getinstance(), xx, yy, My_map.getinstance().addPoint_number % MyMapPointWithTitleView.color_number, true, name, my_angle));
                                My_map.getinstance().addPoint_number++;
                            }
                        }
                    }
                    My_map.getinstance().myMapView.addMapPoints_from_slam(mapPoints_from_slam);
            }
            My_map.getinstance().myMapView.Refresh_show_get_new_map_picture( );
        } catch (Exception e) {
            e.printStackTrace();
        }
    }
}
